﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Threading;
using System.IO;
using System.Diagnostics;
using System.IO.Ports;

namespace R10App
{
    public partial class MainForm : Form
    {
        private JoystickHandler joy = new JoystickHandler();

        // data
        SDataStructure DataStructure = new SDataStructure();

        double GRollOffset = -2.5/180*Math.PI;
        double GPitchOffset = 0;

        // alt zero
        double GPSZeroAlt = 0;
        double BaroZeroAlt = 0;

        // XBee comm
        string GlobalSerialBuffer = "";
        double GlobalCommTimer = 0;
        double GlobalCommGPSTimer = 0;

        Bitmap bitMapSideView;
        Bitmap bitMapTopView;
        Bitmap offscreenSurf = new Bitmap(600, 390);
        Bitmap offscreenSurfTopView = new Bitmap(330, 270);

        FilterMA battPercentFilt = new FilterMA();

        MapLoader mapLoader = new MapLoader();

        public struct SDataStructure
        {
            public int ReceivedPackets;
            public int ReceiveErrors;
            public int RXThreadTicks;

            public int TransmitPackets;
            public int TransmitErrors;

            // data
            // Sensors - I/O Board
            public int ModeActual;
            public int Fail;
            public float Voltage;
            public float PWMOut1;
            public float PWMOut2;
            public float PWMOut3;
            public float PWMOut4;
            public float Sonar;
            public float IRNear;
            public float IRFar;
            public float LoopTime;
            public float LoopTimeMAX;
            public double TotalTime;

            public float MagX;
            public float MagY;
            public float MagZ;
            public float MagYaw;

            // IMU Board
            public float GyroX;
            public float GyroY;
            public float GyroZ;
            public float Roll;
            public float Pitch;
            public float Yaw;
            public float AbsPressure;
            public float DiffPressure;
            public float IMUTemperature;
            public double GPSLatitude;
            public double GPSLongitude;
            public double TargetGPSLatitude;
            public double TargetGPSLongitude;
            public float GPSAltitude;
            public float GPSSatNr;
            public float VelX;
            public float VelY;

            // Other Data
            public int Counter;
            public float AltitudeF;
            public float VertSpeed;
            public int KillCounter;
            
        };

        public MainForm()
        {
            InitializeComponent();

            string[] ports = SerialPort.GetPortNames();
            comboBoxPorts.Items.AddRange(ports);
            if (comboBoxPorts.Items.Count > 0) comboBoxPorts.SelectedIndex = 0;

            battPercentFilt.Init(100, 100);
        }

        private void MainForm_Load(object sender, EventArgs e)
        {
            joy.Init(this.Handle);

            mapLoader.LoadMap("mapj");
            bitMapSideView = (Bitmap)Bitmap.FromFile("sideviewfinal.png");
            bitMapTopView = (Bitmap)Bitmap.FromFile("topviewfinal.png");
        } 

        private void timerGUI_Tick(object sender, EventArgs e)
        {
            // update GUI
            // 0. Serial COM
            if( serialPort1.IsOpen)
            {
                // update Comm health
                GlobalCommTimer += 0.025; // 25ms/tick
                GlobalCommGPSTimer += 0.025; // 25ms/tick
                textBoxCommTimer.Text = string.Format("{0:0.0}", GlobalCommTimer);
                textBoxCommGPSTimer.Text = string.Format("{0:0.0}", GlobalCommGPSTimer); 
                if (GlobalCommTimer < 0.5) textBoxCommTimer.BackColor = Color.Green;
                else if (GlobalCommTimer < 2) textBoxCommTimer.BackColor = Color.Yellow;
                else
                {
                    textBoxCommTimer.BackColor = Color.Red;
                }

                if (GlobalCommGPSTimer < 1.0) textBoxCommGPSTimer.BackColor = Color.Green;
                else if (GlobalCommGPSTimer < 3) textBoxCommGPSTimer.BackColor = Color.Yellow;
                else
                {
                    textBoxCommGPSTimer.BackColor = Color.Red;
                }

                if (GlobalCommTimer > 2) labelCommFailText.Enabled = true;
                else labelCommFailText.Enabled = false;

                try
                {
                    string read = serialPort1.ReadExisting();
                    if (read.Length > 0)
                    {
                        string toWrite = read;
                        toWrite = toWrite.Replace("\n\r", "\r\n");
                        File.AppendAllText(Application.StartupPath + "\\CommFile.txt", toWrite);

                        GlobalSerialBuffer += read; // cat
                        string[] tokens = GlobalSerialBuffer.Split('\r');
                        if (tokens.Length > 1)
                        {
                            for (int tk = 0; tk != tokens.Length; tk++ )
                            {
                                string target = tokens[tk];

                                // Do something
                                string[] splits = target.Split(',');

                                if (splits[0] == "$DATA" && splits.Length == 11)
                                {
                                    // "$DATA,%ld,%d,%0.2f,%0.3f,%0.3f,%0.3f,%0.0f,%0.2f,%0.2f,%d\n\r", df.Counter, (int)df.ActualMode, df.BattVoltage, df.Roll, df.Pitch, df.MagYaw, df.SonicCM, df.AltF, df.VertSpeed, df.KillCounter);
                                    int counter = int.Parse(splits[1]);
                                    int amode = int.Parse(splits[2]);
                                    float voltage = float.Parse(splits[3]);
                                    float roll = float.Parse(splits[4]);
                                    float pitch = float.Parse(splits[5]);
                                    float magyaw = float.Parse(splits[6]);
                                    float sonicCM = float.Parse(splits[7]);
                                    float AltF = float.Parse(splits[8]);
                                    float VertSpeed = float.Parse(splits[9]);
                                    int KillCounter = int.Parse(splits[10]);

                                    DataStructure.Counter = counter;
                                    DataStructure.ModeActual = amode;
                                    DataStructure.Voltage = voltage;
                                    DataStructure.Roll = roll;
                                    DataStructure.Pitch = pitch;
                                    DataStructure.MagYaw = magyaw;
                                    DataStructure.Sonar = sonicCM;
                                    DataStructure.AltitudeF = AltF;
                                    DataStructure.VertSpeed = VertSpeed;
                                    DataStructure.KillCounter = KillCounter;

                                    textBoxRXBox.Text = target;
                                    GlobalCommTimer = 0; // reset timer
                                }
                                if (splits[0] == "$GPS" && splits.Length == 9)
                                {
                                    // "$GPS,%0.3f,%0.3f,%d,%0.8Lf,%0.8Lf,%0.3f,%0.8Lf,%0.8Lf\n\r", df.VelX, df.VelY, (int)df.gpssatCount, df.gpsLongitude, df.gpsLatitude, df.gpsAltitude, df.targetGpsLongitude, df.targetGpsLatitude);
                                    float VelX = float.Parse(splits[1]);
                                    float VelY = float.Parse(splits[2]);
                                    int gpsSatCount = int.Parse(splits[3]);
                                    double gpsLongitude = double.Parse(splits[4]);
                                    double gpsLatitude = double.Parse(splits[5]);
                                    float gpsAlt = float.Parse(splits[6]);
                                    double targetGpsLongitude = double.Parse(splits[7]);
                                    double targetGpsLatitude = double.Parse(splits[8]);

                                    DataStructure.GPSSatNr = gpsSatCount;
                                    DataStructure.GPSLatitude = gpsLatitude;
                                    DataStructure.GPSLongitude = gpsLongitude;
                                    DataStructure.TargetGPSLatitude = targetGpsLatitude;
                                    DataStructure.TargetGPSLongitude = targetGpsLongitude;
                                    DataStructure.VelX = VelX;
                                    DataStructure.VelY = VelY;
                                    DataStructure.GPSAltitude = gpsAlt;

                                    GlobalCommGPSTimer = 0; // reset timer
                                }
                            }
                        }
                        GlobalSerialBuffer = tokens.Last();
                    }
                }
                catch
                {
                }
            }

            // 1. get copy
            SDataStructure copyStruct;
            lock (this)
            {
                copyStruct = DataStructure;
            }

            // 2. set values          
            textBoxKillCounter.Text = string.Format("{0:0}", copyStruct.KillCounter);
            if (copyStruct.KillCounter < 100)
            {
                textBoxKillCounter.BackColor = Color.Red;
            }
            else textBoxKillCounter.BackColor = Color.Green;

            textBoxBattVoltage.Text = string.Format("{0:f1} V", copyStruct.Voltage);           
            textBoxCounter.Text = string.Format("{0}", copyStruct.Counter);
            textBoxSonarM.Text = string.Format("{0:f2}", copyStruct.Sonar/100);
            textBoxIRFar.Text = string.Format("{0:f2}", copyStruct.IRFar/100);
            
            textBoxLoopMS.Text = string.Format("{0:f2} ms", copyStruct.LoopTime);
            textBoxLoopMAXMS.Text = string.Format("{0:f2} ms", copyStruct.LoopTimeMAX);
            textBoxTotalTime.Text = string.Format("{0:f2} s", copyStruct.TotalTime);

            textBoxMagYaw.Text = string.Format("{0:f3}", copyStruct.MagYaw * 180 / Math.PI);

            textBoxGyroX.Text = string.Format("{0:f2} °/s", copyStruct.GyroX * 180 / Math.PI);
            textBoxGyroY.Text = string.Format("{0:f2} °/s", copyStruct.GyroY * 180 / Math.PI);
            textBoxGyroZ.Text = string.Format("{0:f2} °/s", copyStruct.GyroZ * 180 / Math.PI);

            textBoxRoll.Text = string.Format("{0:f2} °", copyStruct.Roll * 180 / Math.PI);
            textBoxPitch.Text = string.Format("{0:f2} °", copyStruct.Pitch * 180 / Math.PI);
            textBoxYaw.Text = string.Format("{0:f2} °", copyStruct.Yaw * 180 / Math.PI);

            textBoxPressureABS.Text = string.Format("{0:f3} kPa", copyStruct.AbsPressure / 1000);
            textBoxPressureDiff.Text = string.Format("{0:f0} Pa", copyStruct.DiffPressure);
            textBoxTemperature.Text = string.Format("{0:f2} °C", copyStruct.IMUTemperature);
            double baroAltitude = 44330 * (1 - Math.Pow((copyStruct.AbsPressure / 101325), 1 / 5.255));
            textBoxAltitudeBaro.Text = string.Format("{0:f2} m", baroAltitude);
            double pitotSpeed = Math.Sqrt(2 * Math.Abs(copyStruct.DiffPressure) / 1.22521);
            if (copyStruct.DiffPressure < 0)
            {
                pitotSpeed = -pitotSpeed;
            }
            textBoxAirSpeed.Text = string.Format("{0:f2} kph", pitotSpeed * 3.6);
            textBoxGPSSatNr.Text = string.Format("{0:f1} ", copyStruct.GPSSatNr);
            if (copyStruct.GPSSatNr < 3) textBoxGPSSatNr.BackColor = Color.Red;
            else if (copyStruct.GPSSatNr < 4) textBoxGPSSatNr.BackColor = Color.Yellow;
            else textBoxGPSSatNr.BackColor = Color.Green;
            textBoxLongitude.Text = string.Format("{0:f8} °", copyStruct.GPSLongitude);
            textBoxLatitude.Text = string.Format("{0:f8} °", copyStruct.GPSLatitude);
            textBoxAltitudeGPS.Text = string.Format("{0:f2} m", copyStruct.GPSAltitude);
            textBoxVelX.Text = string.Format("{0:f2} m/s", copyStruct.VelX);
            textBoxVelY.Text = string.Format("{0:f2} m/s", copyStruct.VelY);
            textBoxAltitudeF.Text = string.Format("{0:f2} m", copyStruct.AltitudeF);
            textBoxVertSpeed.Text = string.Format("{0:f2} m", copyStruct.VertSpeed);

            // alt
            textBoxGPSAltZ.Text = string.Format("{0:f1} m", copyStruct.GPSAltitude - GPSZeroAlt);
            textBoxBaroAltZ.Text = string.Format("{0:f1} m", copyStruct.AltitudeF - BaroZeroAlt);
            textBoxVertSpeed2.Text = string.Format("{0:f1} m/s", copyStruct.VertSpeed);

            textBoxTgtLongitude.Text = string.Format("{0:f6} °", copyStruct.TargetGPSLongitude);
            textBoxTgtLatitude.Text = string.Format("{0:f6} °", copyStruct.TargetGPSLatitude);
            double percent = GetBatteryPercentage(copyStruct.Voltage);
            double percentF = battPercentFilt.AddPoint(percent);
            textBoxBatteryPercent.Text = string.Format("{0:f1}%", percentF);
            if (percentF < 20) textBoxBatteryPercent.ForeColor = Color.Red;
            else if (percentF < 40) textBoxBatteryPercent.ForeColor = Color.Yellow;
            else textBoxBatteryPercent.ForeColor = Color.Lime;


            // offsets
            textBoxRollOff.Text = string.Format("{0:f2} °", GRollOffset * 180 / Math.PI);
            textBoxPitchOff.Text = string.Format("{0:f2} °", GPitchOffset * 180 / Math.PI);

            // update mode
            int mode = (int)copyStruct.ModeActual;
            switch (mode)
            {
                case 0: //off
                    pictureOff.BackColor = Color.Lime;
                    pictureRate.BackColor = Color.Red;
                    pictureRollPitch.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Red;
                    pictureAutoMode.BackColor = Color.Red;
                    buttonOff.Enabled = true;
                    buttonRateMode.Enabled = true;
                    buttonRollPitchMode.Enabled = true;
                    buttonAltMode.Enabled = true;
                    buttonAutoMode.Enabled = true;
                    break;

                case 1: // RateMode
                    pictureOff.BackColor = Color.Red;
                    pictureRate.BackColor = Color.Lime;
                    pictureRollPitch.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Red;
                    pictureAutoMode.BackColor = Color.Red;
                    buttonOff.Enabled = true;
                    buttonRateMode.Enabled = false;
                    buttonRollPitchMode.Enabled = false;
                    buttonAltMode.Enabled = false;
                    buttonAutoMode.Enabled = false;
                    break;

                case 2: // RollPitch Mode
                    pictureOff.BackColor = Color.Red;
                    pictureRate.BackColor = Color.Red;
                    pictureRollPitch.BackColor = Color.Lime;
                    pictureAltMode.BackColor = Color.Red;
                    pictureAutoMode.BackColor = Color.Red;
                    buttonOff.Enabled = true;
                    buttonRateMode.Enabled = false;
                    buttonRollPitchMode.Enabled = false;
                    buttonAltMode.Enabled = false;
                    buttonAutoMode.Enabled = false;
                    break;

                case 3: // Alt Mode
                    pictureOff.BackColor = Color.Red;
                    pictureRate.BackColor = Color.Red;
                    pictureRollPitch.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Lime;
                    pictureAutoMode.BackColor = Color.Red;
                    buttonOff.Enabled = true;
                    buttonRateMode.Enabled = false;
                    buttonRollPitchMode.Enabled = false;
                    buttonAltMode.Enabled = false;
                    buttonAutoMode.Enabled = false;
                    break;

                case 4: // Auto Mode
                    pictureOff.BackColor = Color.Red;
                    pictureRate.BackColor = Color.Red;
                    pictureRollPitch.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Red;
                    pictureAutoMode.BackColor = Color.Lime;
                    buttonOff.Enabled = true;
                    buttonRateMode.Enabled = false;
                    buttonRollPitchMode.Enabled = false;
                    buttonAltMode.Enabled = false;
                    buttonAutoMode.Enabled = false;
                    break;

                default:
                    pictureOff.BackColor = Color.Red;
                    pictureRate.BackColor = Color.Red;
                    pictureRollPitch.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Red;
                    pictureAutoMode.BackColor = Color.Red;
                    break;
            }

            // 3. read inputs (joy), TODO!!!
            JoyState js =joy.Poll();
            if (js == null) return;

            // buttons
            if (js.Button1)
            {
                buttonModeSelect_Click(buttonOff, new EventArgs());
            }
            if (js.Button2)
            {
                buttonModeSelect_Click(buttonOff, new EventArgs());
                Thread.Sleep(200);
                buttonModeSelect_Click(buttonRollPitchMode, new EventArgs());
                Thread.Sleep(200);
                buttonModeSelect_Click(buttonRollPitchMode, new EventArgs());
            }
            if (js.Button3)
            {
                buttonModeSelect_Click(buttonOff, new EventArgs());
                Thread.Sleep(200);
                buttonModeSelect_Click(buttonAutoMode, new EventArgs());
                Thread.Sleep(200);
                buttonModeSelect_Click(buttonAutoMode, new EventArgs());
            }

            float thrRef = (float)js.Throttle;
            float rollRef = (float)js.AxisX;
            float pitchRef = (float)-js.AxisY;
            float yawRef = (float)js.AxisZ;

            // deadzone
            if (Math.Abs(rollRef) < 0.2) rollRef = 0;
            if (Math.Abs(pitchRef) < 0.2) pitchRef = 0;
            if (Math.Abs(yawRef) < 0.2) yawRef = 0;

            // scale
            rollRef = rollRef / 3;
            pitchRef = pitchRef / 3;

            // ADD offsets
            if (js.Hat1 == 90) GRollOffset += 0.0002;
            if (js.Hat1 == 270) GRollOffset -= 0.0002;
            if (js.Hat1 == 0) GPitchOffset -= 0.0002;
            if (js.Hat1 == 180) GPitchOffset += 0.0002;
            rollRef += (float)GRollOffset*2;
            pitchRef += (float)GPitchOffset*2;

            // add yaw offset (only in heading hold modes!)
            if (mode >= 3)
            {
                float yawOffset = hScrollBarYawOffset.Value / 100.0f;
                yawRef = yawRef + yawOffset;
            }

            if (yawRef < -1) yawRef += 2;
            if (yawRef > 1) yawRef -= 2;

            //if (mode >= 3) // altitude mode
            //{
            //   textBoxThrottleSet.Text = string.Format("{0:0.00 m}", (thrRef-0.1)/0.9 * 3);
            //}
            //else
            {
                textBoxThrottleSet.Text = string.Format("{0:0.00}", thrRef);
            }
            textBoxRollRef.Text = string.Format("{0:0.00}", rollRef);
            textBoxPitchRef.Text = string.Format("{0:0.00}", pitchRef);
            textBoxYawRef.Text = string.Format("{0:0.00}", yawRef);

             
            // 4. send messages
            bool silentMode = false;
            if (!silentMode)
            {
                // serial comm.
                if (serialPort1.IsOpen)
                {
                    string toSend = "$REF,";
                    toSend += string.Format("{0:0.000},", rollRef);
                    toSend += string.Format("{0:0.000},", pitchRef);
                    toSend += string.Format("{0:0.000},", yawRef);
                    toSend += string.Format("{0:0.000},", thrRef);
                    toSend += "\r";
                    try
                    {
                        serialPort1.Write(toSend);
                    }
                    catch
                    {
                    }
                }
            }

            // 6.draw Map 
            Graphics g = Graphics.FromImage(offscreenSurf);
            g.DrawImageUnscaled(mapLoader.Getmap(), 0, 0);

            // direction
            double angle = copyStruct.MagYaw; // rads
            double AX = Math.Sin(angle) * 25;
            double AY = -Math.Cos(angle) * 25;

            // Draw position!
            double Lon = copyStruct.GPSLongitude;
            double Lat = copyStruct.GPSLatitude;
            int PX, PY;
            mapLoader.LL2Pix(Lat, Lon, out PX, out PY);
            if (Math.Abs(PX) < 1000 && Math.Abs(PY) < 1000)
            {
                g.FillEllipse(Brushes.Red, (float)PX - 5, (float)PY - 5, 10, 10);
                g.DrawLine(Pens.DarkCyan, (float)PX, (float)PY, (float)(PX + AX), (float)(PY + AY));
            }

            // Draw target position
            Lon = copyStruct.TargetGPSLongitude;
            Lat = copyStruct.TargetGPSLatitude;
            mapLoader.LL2Pix(Lat, Lon, out PX, out PY);
            if (Math.Abs(PX) < 1000 && Math.Abs(PY) < 1000)
            {
                Pen pp = new Pen(Color.Blue, 3);
                g.DrawEllipse(pp, (float)PX - 8, (float)PY - 8, 16, 16);
            }

            // copy to image
            Graphics targetG = panelMap.CreateGraphics();
            targetG.DrawImage(offscreenSurf, 0, 0);

            // 7.draw vert 
            g = Graphics.FromImage(offscreenSurfTopView);
            g.Clear(Color.Black);
            g.TranslateTransform(-bitMapTopView.Width / 2, -bitMapTopView.Height / 2, System.Drawing.Drawing2D.MatrixOrder.Append);
            g.RotateTransform((float)(angle / Math.PI * 180), System.Drawing.Drawing2D.MatrixOrder.Append);
            g.TranslateTransform(bitMapTopView.Width / 2, bitMapTopView.Height / 2, System.Drawing.Drawing2D.MatrixOrder.Append);            
            g.DrawImageUnscaled(bitMapTopView, 0, 0);
            g.ResetTransform();
            // draw arrows
            int x0 = bitMapTopView.Width / 2;
            int y0= bitMapTopView.Height / 2;
            int xDist = (int)(copyStruct.VelY * 100);
            int yDist = (int)(-copyStruct.VelX * 100);
            Pen p = new Pen(Color.Purple, 5);
            g.DrawLine(p, x0, y0, x0 + xDist, y0);
            g.DrawLine(p, x0 + xDist, y0, x0 + xDist - Math.Sign(xDist)*10, y0+10);
            g.DrawLine(p, x0 + xDist, y0, x0 + xDist - Math.Sign(xDist) * 10, y0 - 10);
            g.DrawLine(p, x0, y0, x0, y0+yDist);
            g.DrawLine(p, x0, y0 + yDist, x0 + 10, y0 + yDist - Math.Sign(yDist) * 10);
            g.DrawLine(p, x0, y0 + yDist, x0 - 10, y0 + yDist - Math.Sign(yDist) * 10);

            // draw ref. heading
            if (mode >= 3)
            {
                double yawRefRads = yawRef*Math.PI;
                xDist = (int)(Math.Sin(yawRefRads)*100);
                yDist = (int)(-Math.Cos(yawRefRads) * 100);
                p = new Pen(Color.Blue, 3);
                g.DrawLine(p, x0, y0, x0 + xDist, y0 + yDist);
            }

            targetG = panelVerticalForces.CreateGraphics();
            targetG.DrawImage(offscreenSurfTopView, 0, 0);
        }

        private void buttonModeSelect_Click(object sender, EventArgs e)
        {
            int mode = 0;

            if (sender == buttonOff) mode = 0;
            else if (sender == buttonRateMode) mode = 1;
            else if (sender == buttonRollPitchMode) mode = 2;
            else if (sender == buttonAltMode) mode = 3;
            else if (sender == buttonAutoMode) mode = 4;

            if (serialPort1.IsOpen)
            {
                string toSend = "$MODE,";
                toSend += string.Format("{0:0},", mode);
                toSend += "\r";
                serialPort1.Write(toSend);
            }
        }     

        private void buttonOpenCOM_Click(object sender, EventArgs e)
        {
            try
            {
                serialPort1.PortName = (string)comboBoxPorts.SelectedItem;
                serialPort1.Open();
                buttonOpenCOM.Enabled = false;
                buttonOpenCOM.Text = "Close";
                timerGUI.Enabled = true;
            }
            catch
            {
            }
        }

        private void buttonZeroOffsets_Click(object sender, EventArgs e)
        {
            SDataStructure copyStruct;
            lock (this)
            {
                copyStruct = DataStructure;
            }
            GRollOffset = copyStruct.Roll;
            GPitchOffset = copyStruct.Pitch;
        }

        private void panelMap_MouseClick(object sender, MouseEventArgs e)
        {
            double LLx, LLy;
            mapLoader.Pix2LL(e.X, e.Y, out LLy, out LLx);

            if (serialPort1.IsOpen)
            {
                string toSend = "$TGT,";
                toSend += string.Format("{0:f8},{1:f8}", LLy, LLx);
                toSend += "\r";
                serialPort1.Write(toSend);
            }
        }

        private double GetBatteryPercentage(double voltage)
        {
            double percent = 0;
            if( voltage > 11.9 ) percent = 100;
            else if( voltage < 9 ) percent = 0;
            else
            {
                double a1 =       99.57;
                double b1 =       11.85;
                double c1 =       1.033;
                double a2 =       6.968;
                double b2 =       11.33;
                double c2 =       0.228;
                double a3 =       13.17;
                double b3 =          11;
                double c3 =      0.2414;
                double x = voltage;
                percent = a1 * Math.Exp(-Math.Pow(((x - b1) / c1),2)) + a2 * Math.Exp(-Math.Pow(((x - b2) / c2),2)) + a3 * Math.Exp(-Math.Pow(((x - b3) / c3),2));
            }

            return percent;
        }

        private void button1_Click(object sender, EventArgs e)
        {
            lock (this)
            {
                GPSZeroAlt = DataStructure.GPSAltitude;
                BaroZeroAlt = DataStructure.AltitudeF;
            }
        }
    }
}
